/*
 * Copyright (c) 2010, Jim Hollinger
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of Jim Hollinger nor the names of its contributors
 *     may be used to endorse or promote products derived from this
 *     software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "debughelper/debughelper.h"
#include "dtime/dtime.h"
#include "kmlio/kmlreader.h"
#include "kmlstatus.h"
#include "kmlinterface.h"


using namespace kmljgp;


// pthread to start kml api location updates
void *kmljgp::kml_start_pthread(void *arg) {
	KmlInterface *object = (KmlInterface *) arg;
	if (object != NULL) {
		object->run();
		printf("kml_start_pthread returning ...\n");
	}
	return arg;
}


KmlInterface::KmlInterface() {
	thread_ID.p         = NULL;
	thread_ID.x         = 0;
	pthread_mutex_init(&status_mutex, NULL);
	status              = new KmlStatus();
	kmlReader           = new kmlio::KmlReader();

	period_s            = 1.0;

	stop_flag           = false;
}

KmlInterface::~KmlInterface() {
	stop();

	// wait for kml reader thread to stop
	pthread_join(thread_ID, NULL);
	dtime::delaySec(period_s);
	printf("~KmlInterface exiting ...\n");

	delete kmlReader;
	kmlReader = NULL;

    delete status;
	status = NULL;

	pthread_mutex_destroy(&status_mutex);
}

bool KmlInterface::init(const std::string &kml_filename, bool loopCoordinates_flag) {
	bool flag = false;

	if ((status == NULL) || (kmlReader == NULL)) {
		fprintf(stderr, "KmlInterface.init: All objects not created\n");
	} else {
		flag = kmlReader->parse(kml_filename);
		kmlReader->loopCoordinates_flag = loopCoordinates_flag;
	}

	return flag;
}

bool KmlInterface::start(double interfaceRate_hz) {
	bool flag = false;

	// compute update interval for kml coordinates, used in run() method
	if (interfaceRate_hz < 0.001) {
		interfaceRate_hz = 0.001;
	}
	period_s  = 1.0 / interfaceRate_hz;

	// create thread for kml api location updates
	int rc = pthread_create(&thread_ID, NULL, kml_start_pthread, this);
	if (rc == 0) {
		printf("KmlInterface started\n");
		flag = true;
	} else {
		fprintf(stderr, "KmlInterface.start: pthread_create failed: %d\n", rc);
	}

	return flag;
}

void KmlInterface::run() {
	double latitude_degN      = 0.0;
	double longitude_degE     = 0.0;
	double altitude_m         = 0.0;
	bool   altitudeValid_flag = false;

	// loop
	while (!stop_flag) {
		dtime::delaySec(period_s);
		if (!isStop()) {
			kmlReader->getNextCoordinate(
                &latitude_degN,
                &longitude_degE,
                &altitude_m,
                &altitudeValid_flag);
			pthread_mutex_lock(&status_mutex);
			{
				status->time_s              = dtime::getTime_s();
				status->latitude_degN       = latitude_degN;
				status->longitude_degE      = longitude_degE;
				status->altitude_m          = altitude_m;
				status->altitudeValid_flag  = altitudeValid_flag;
			}
			pthread_mutex_unlock(&status_mutex);
		}
	}
}

void KmlInterface::stop() {
	if (!stop_flag) {
		stop_flag = true;
		printf("KmlInterface.stop stopping ...\n");
	}
}

bool KmlInterface::isStop() const {
	return stop_flag;
}

void KmlInterface::getStaus(KmlStatus *dst_status) {

	if ((dst_status != NULL) && !isStop()) {
		pthread_mutex_lock(&status_mutex);
		{
			*dst_status = *status;
		}
		pthread_mutex_unlock(&status_mutex);
	}
}
